/*=+--+=#=+--     Unmanned Aerial System Management Software      --+=#=+--+=#*\
|          Copyright (C) 2011 Regents of the University of Colorado.           |
|                             All Rights Reserved.                             |

     This program is free software: you can redistribute it and/or modify
     it under the terms of the GNU General Public License version 2 as
     published by the Free Software Foundation.

     This program is distributed in the hope that it will be useful,
     but WITHOUT ANY WARRANTY; without even the implied warranty of
     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
     GNU General Public License for more details.

     You should have received a copy of the GNU General Public License
     along with this program.  If not, see <http://www.gnu.org/licenses/>.

            Jack Elston                       Cory Dixon                        
|           elstonj@colorado.edu              dixonc@colorado.edu              |
|                                                                              |
\*=+--+=#=+--                 --+=#=+--+=#=+--                    --+=#=+--+=#*/

/***********************************************************************
 *
 * FILENAME:
 * gpsWaypoint.cxx
 *
 * PURPOSE:
 * Input and output of GPS waypoint lists as defined by CloudCap Tech
 * for the Piccolo.
 *
 * CREATED:
 * 02/2005 by Cory Dixon
 *
 * LAST MODIFIED:
 * $Author: dixonc $
 * $Date: 2005/09/06 04:04:50 $
 * $Revision: 1.4 $
 *
 ***********************************************************************/

#include <errno.h>
#include <fcntl.h>
#include <unistd.h>
#include <sys/types.h>		// for open(), read(), write(), close()
#include <sys/stat.h>		 // for open(), read(), write(), close()
 
#include <iostream>
#include <fstream>
#include <sstream>
#include <iomanip>
#include <stdio.h>
#include <math.h>
 
#include "gpsWaypoint.h"
#include "command.h"

// Returns the left based index of the most significant bit which is set
uint32_t FindFirstBitSet(uint32_t r);

GPSWaypointPlan::GPSWaypointPlan()
{
	//cout << "GPSWaypointPlan::GPSWaypointPlan()" << endl;
	GPSWaypointPlan::clear_list();
	for( int i=0; i < NUM_WP_POINTS; i++ ) {
		Plan[i].Next = 0xFF;
		Plan[i].index = 0xFF;
	}
}

GPSWaypointPlan::~GPSWaypointPlan()
{
	GPSWaypointPlan::clear_list();
}

bool GPSWaypointPlan::save_to_file(String file, uint8_t startInd)
{

	if( startInd >= NUM_WP_POINTS || !Plan[startInd].IsPointValid() )
	{
		cout << "!!! not a valid startInd in current plan for saving to file" << endl;
		return false;
	}

	WPMask_t Mask;
	int numPoints = GetPointsInPlan(Mask, startInd);
	if( numPoints <= 0 )
	{
		cout << "!!! no points in waypoint plan" << endl;
		return false;
	}

	//-- find the very begining of the flight plan, or the lowest number --//
	startInd = find_first_in_plan(startInd);

	cout << "Saving waypoint plan file       : ";
	fflush(stdout);

	ofstream of(file.c_str(), ios::out);
	if( !of )	{
		//perror("Could not open waypoint file");
		cout << ".......... [error]" << endl;
		return false;
	}

	//-- output header --//
	// we need a point to determine header size
	GPSWaypoint_t point = Plan[ startInd ];
	of << "# Flight plan file" << endl
	   << "# Index, Next, Lat[deg], Lon[deg], Alt[m]";
	if( point.validElements >= 17 )
		of << ", WindInterval[km], Preturn, Lights, Slope, Land, Parachute, Drop, Camera, OrbitDirection, OrbitRadius[m], OrbitTime[s], User";
	if( point.validElements >= 20 )
		of << ", OrbitAboveAlt, OrbitBelowAlt, Hover";
	of << endl;

	//-- now loop through plan and save to file --//
	uint8_t curr_ind = startInd;
	while( GetWPMaskBit(Mask, curr_ind) )
	{
		of << Plan[curr_ind] << endl;

		ClearWPMaskBit(Mask, curr_ind);
		curr_ind = Plan[curr_ind].Next;
	}

	of.close();
	cout << "# points =" << setw(3) << numPoints << " ";
	cout << " [done]" << endl;
	return true;
}

bool GPSWaypointPlan::read_from_file(String file)
{
	GPSWaypoint_t point;
	cout << "Loading waypoint plan file      : ";

	ifstream inputFile(file.c_str(), ios::in);
	if( !inputFile )	{
		perror("Could not open GPS waypoint file");
		return false;
	} else {

		char line[256];
		bool comment = 1;
		do {
			if( inputFile.peek() == '#') {
				if( !inputFile.getline(line,256)) {	
					perror("could not read line");
					return false;
				}
			} else
				comment = false;
		} while(comment);

		// find out how many arguments there are for 
		// version number
		std::string tmpLine;
		std::string tmpElement;

		// read one line to get number of elements
		getline(inputFile, tmpLine );
		std::stringstream is(tmpLine);
		point.validElements = 0;
		while( is >> tmpElement ) point.validElements++;

		int numPoints = 0;
		int start_ind = -1;

		// now start processing, using same line
		do {
			std::stringstream is1(tmpLine);
			if( is1 >> point ) {
				if( !add_waypoint(point) ) printf("!!==== did not load point\n");
				else {
					numPoints++;
					if( start_ind < 0 )
						start_ind = point.index;	
				}
			} //else printf("!!==== did not read point\n");

		} while( getline(inputFile, tmpLine ) );

		inputFile.close();

		cout << "# points =" << setw(3) << numPoints << " ";

		// check to see if there is a closed loop
		if( !CheckFlightPlanClosure( start_ind ) ) {
			printf(" !!===== FILE DOES NOT HAVE A CLOSING PLAN\n");
		} else
			cout << " [done]" << endl;
	}
	return true;
}

bool GPSWaypointPlan::read_from_cmd(uint8_t * buf)
{
	GPSWaypoint_t point;
	int numPoints = (CMD_NUM(buf)/sizeof(Waypt_t));

	//cout << "Loading " << numPoints << " waypoints from command: ";

	Waypt_t * waypoint = (Waypt_t *)CMD_DATAPTR(buf);

	for(int i = 0; i < numPoints; i++) {

		//cout << i << "-> num=" << (int)waypoint[i].number << " next=" << (int)waypoint[i].next << endl;

		float orbitRad = abs( waypoint[i].orbitRad );
		bool orbitDir = ( waypoint[i].orbitRad < 0 ? 1 : 0 );

		point.Lat            = waypoint[i].pos.lat;
		point.Lon            = waypoint[i].pos.lon;
		point.Alt            = waypoint[i].pos.alt;
		point.OrbitRadius    = orbitRad;
		point.OrbitDirection = orbitDir;
		point.Next           = waypoint[i].next;
		point.index          = waypoint[i].number;
		point.validElements  = 20; 

		if( !add_waypoint(point) ) printf("!!==== did not load point\n");
		//else numPoints ++;
	}

	//cout << " # points: " << numPoints;

	if( !CheckFlightPlanClosure( point.index ) ) {
		printf("!!===== CMD DOES NOT HAVE A CLOSING LOOP\n");
	}// else cout << " [done] " << endl;

	return true;
}

/* when you add a waypoint, if the plan has a waypoint currently defined there,
 * then the plan that contained the pre-existing point is cleared.
 */
bool GPSWaypointPlan::add_waypoint(GPSWaypoint_t &point)
{
	//cout << "GPSWaypointPlan::add_waypoint - point.index=" << (int)point.index  << " point.next=" << (int)point.Next <<  endl;
	if( point.index >= NUM_WP_POINTS || !point.IsPointValid() ) {
		cout << "GPSWaypointPlan::add_waypoint -- point is not valid" << endl;
		return false;
	}

	WPMask_t Mask;
	if( GetPointsInPlan(Mask, point.index) > 0 )  {
		//cout << "GPSWaypointPlan::add_waypoint -- clearing previous defined flight plan defined at " << (int)point.index << endl;
		ClearPointsFromPlan(Mask);
	}

	//cout << numWaypoints << ": " << *point << endl;

	Plan[ point.index ] = point;
	numWaypoints++;

	return true;
}

bool GPSWaypointPlan::set_waypoint(GPSWaypoint_t &point)
{ 
	if( point.index >= NUM_WP_POINTS || !point.IsPointValid() )
		return false;

	Plan[point.index] = point;
	return true;
}

GPSWaypoint_t GPSWaypointPlan::get_waypoint(uint8_t ind) 
{ 
	if( ind < NUM_WP_POINTS )
		return Plan[ind];

	GPSWaypoint_t point;
	return point;
}

double GPSWaypointPlan::dist_deg2(int wpInd, double lon, double lat)
{
	double xdiff, ydiff;

	if( wpInd >= NUM_WP_POINTS )
		return INFINITY;

	if( Plan[ wpInd ].index == wpInd ) {
		xdiff = Plan[ wpInd ].Lon - lon;
		ydiff = Plan[ wpInd ].Lat - lat;
		return xdiff*xdiff + ydiff*ydiff;
	}

	return INFINITY;
}

int GPSWaypointPlan::find_closest(double lon, double lat)
{
	int i=0;
	double xdiff, ydiff, diff, diffMin = INFINITY; 
	int minInd = -1;

	for( i=0; i < NUM_WP_POINTS; i++ ) {
		if( Plan[i].index == i && Plan[i].IsPointValid() ) {
			xdiff = Plan[i].Lon - lon;
			ydiff = Plan[i].Lat - lat;
			diff = xdiff*xdiff + ydiff*ydiff;
		
			if( diff < diffMin ) {
				diffMin = diff;
				minInd = i;
			}	
		}
	}

	return minInd;
}

int32_t GPSWaypointPlan::find_first_in_plan(int startInd)
{
	WPMask_t Mask;
	int numPoints = GetPointsInPlan(Mask, startInd);
	if( numPoints <= 0 ) {
		//cout << "!!! no points in waypoint plan" << endl;
		return -1;
	}

	//-- find the very begining of the flight plan, or the lowest number --//
	int prev_ind = FindFirstWPMaskBitSet(Mask);
	int curr_ind;
	do {
		curr_ind = prev_ind;
		ClearWPMaskBit(Mask, curr_ind);
		prev_ind = FindPreviousWPPoint((uint8_t)curr_ind);
		if( prev_ind >= 0 && !GetWPMaskBit(Mask, prev_ind) ){
			curr_ind = prev_ind;
			prev_ind = -1;
		}
	} while( prev_ind >= 0 );

	return curr_ind;
}

/*
void GPSWaypointPlan::close_loop(int loopInd)
{
	printf("GPSWaypointPlan::close_loop\n");
}
void GPSWaypointPlan::remove_last_waypoint()
{
	cout << " GPSWaypointPlan::remove_last_waypoint() " << endl;
	numWaypoints--;
}
*/

void GPSWaypointPlan::clear_list()
{
	numWaypoints = 0;
	for( int i=0; i < NUM_WP_POINTS; i++)
		Plan[i].clear();
}


/*! Verify the flight plan closes and all points are valid.
	\param Plan is an array of waypoints NUM_WP_POINTS long.
	\param Start is an index of a waypoit in the flight plan.
	\return true if the plan closes, else false.*/
bool GPSWaypointPlan::CheckFlightPlanClosure(uint8_t Start)
{
	WPMask_t WPMask;
	uint8_t Next = Start;
	uint8_t Temp;
	uint8_t NumPoints = 0;

	ClearAllWPMaskBits(WPMask);	// Init all bits to zero

	while(true)
	{
		// Check waypoint validity
		if(Next >= NUM_WP_POINTS)
			return false;

		// Check for closure by determining if we've already run into the
		//   next point.  That indicates a plan which is closed.
		if(GetWPMaskBit(WPMask, Next))
			break;

		SetWPMaskBit(WPMask, Next);	// Indicate this point valid
		NumPoints++;				// Track number of points

		Temp = Plan[Next].Next;		// Get the next point in the circuit

		// Check to make sure that the plan doesn't close about a single point
		if(Temp == Next)
			return false;

		Next = Temp;

	}// while

	if(NumPoints > 1)
		return true;	// Must have at least two points for a closed plan
	else
		return false;
}// CheckFlightPlanClosure


/*! Get all the points that are on the flight plan which includes the Next
    waypoint and return them as bits in a mask.  This function will find all
    the points which link to this one, no matter their arrangement
    \param Plan points to a flight plan array.
    \param Mask is the bit mask to receive a bit for each point.
    \param Point is the index of the seed waypoint to start the search.
    \return The total number of points found.*/
uint8_t GPSWaypointPlan::GetPointsInPlan(WPMask_t Mask, uint8_t Point)
{
	uint32_t i, j;
	uint8_t Num = 0;
		
	// Initialize all the bits to zero
	ClearAllWPMaskBits(Mask);

	// Search the list forward, this is the easy part	
	while(Point < NUM_WP_POINTS)
	{
		if(Plan[Point].Next < NUM_WP_POINTS && Plan[Point].index == Point)
		{
		//cout <<"GPSWaypointPlan::GetPointsInPlan : next=" << (int)Plan[Point].Next << " index=" << (int)Plan[Point].index << endl;
			if(GetWPMaskBit(Mask, Point))
				break;
			else
			{
				SetWPMaskBit(Mask, Point);	// Log this point
				Num++;						// Count number of points
			}
				
		}// If valid waypoint

		// Go forward through the list
		Point = Plan[Point].Next;
		
	}// While still searching points forward
			

	// Now search for antecedent points, there can be multiples of these
	//   for each point, so the search is much more extensive.
	for(i = 0; i < NUM_WP_POINTS; i++)
	{
		if(GetWPMaskBit(Mask, i))
		{
			for(j = 0; j < NUM_WP_POINTS; j++)
			{
				if((Plan[j].Next == i) && (i != j))
				{
					if(!GetWPMaskBit(Mask, j))
					{
						SetWPMaskBit(Mask, j);	// Log this point
						Num++;					// Count number of points
						
						// We may need to go back and look for
						//   links to our newly found point
						if(j < i)
						{
							// Backup outer loop to one less than inner loop
							//   (recall outer loop will increment at top)
							i = (uint32_t)(j-1);
							
							// Break out of the inner for loop and go back to
							//   the outer loop
							break;
							
						}// If found a new point that we had previously passed
					}// if we have not yet found this point
				}// if this point links to i
			}// for all possible points which link to i
		}// If this point (i) is in the plan
	}// for all possible points so far
	
	return Num;

}// GetPointsInPlan


/*! Clear the requested waypoints from the flight plan array.  For every bit
	set in the Mask the corresponding waypoint is cleared from the passed array.
	\param Plan is an array of waypoints.
	\param Mask contains a bit for each waypoint to be cleared.*/
void GPSWaypointPlan::ClearPointsFromPlan(WPMask_t Mask)
{
	int32_t i = FindFirstWPMaskBitSet(Mask);

	while(i >= 0 && i < NUM_WP_POINTS)
	{
		Plan[i].clear();
		numWaypoints--;
		//cout << "GPSWaypointPlan::ClearPointsFromPlan : point=" << Plan[i] << endl;
		i = FindNextWPMaskBitSet(Mask, (uint32_t)i);
	}// Until the points are gone
}// ClearPointsFromPlan


/*! Find the waypoint index which precedes the passed waypoint index.  This
  is done by searching the waypoint list looking for the waypoint whose Next
  value matches the passed waypoint index.  It is possible to have multiple
  waypoints preceding the passed one, (the waypoint list is linked singly)
  in which case the returned waypoint index will be the highest index that
  is less than the passed waypoint index.
  \param Plan is an array of waypoints NUM_FP_POINTS long.
  \param Point is the index of the waypoint whose preceding index should be
       returned.
  \return The index of the preceding waypoint, or -1 if no such waypoint
      exists. */
int32_t GPSWaypointPlan::FindPreviousWPPoint(uint8_t Point)
{
  int32_t i = Point;
  int32_t Num = 0;

  if(Point >= NUM_WP_POINTS)
  {
    printf("\n\rInvalid argument to Find Previous WP");
    return -1;
  }

  // Make optimun guess designed to reduce the number of loop
  //   iterations.  Most likely guess is point just before this one.
  i--;

  while(Num < NUM_WP_POINTS)
  {
    // Watch for wrap of the array
    if(i < 0)
      i = NUM_WP_POINTS - 1;

    if(Plan[i].Next < NUM_WP_POINTS)
    {
      // Is this one the previous one?
      if(Plan[i].Next == Point)
        return i;

    }// If this point is valid

    Num++;  // Count number of iterations
    i--;  // Go to next test point

  }// While still points to check

  return -1;  // No previous point

}// FindPreviousWPPoint


/*! Set the bit in the mask corresponding to the passed waypoint index.
	\param Mask contains a bit for each waypoint to be cleared.
	\param Point is the index of the bit to set. */
void SetWPMaskBit(WPMask_t Mask, uint32_t Point)
{
	uint32_t Word;

	if(Point < NUM_WP_POINTS)
	{
		Word = Point/8;		// Which word of the mask we will be using
		Point -= Word*8;	// Which bit of the word we will be setting

		Mask[Word] |= (uint8_t)(0x80>>Point);
	}// If a valid point
}// SetWPMaskBit


/*! Clear the bit in the mask corresponding to the passed waypoint index.
	\param Mask contains a bit for each waypoint to be cleared.
	\param Point is the index of the bit to clear. */
void ClearWPMaskBit(WPMask_t Mask, uint32_t Point)
{
	uint32_t Word;

	Word = Point/8;		// Which word of the mask we will be using
	Point -= Word*8;	// Which bit of the word we will be setting

	if(Word < WP_MASK_BYTES) // If a valid point
		Mask[Word] &= (uint8_t)(~(0x80>>Point));

}// ClearWPMaskBit


/*! Return the status of the requested bit in the flight plan mask.
	\param Mask is a flight plan mask containing status bits.
	\param Point is the index of the bit whose status will be returned.
	\return true if the bit is set, else false. */
bool GetWPMaskBit(const WPMask_t Mask, uint32_t Point)
{
	uint32_t Word;

	if(Point < NUM_WP_POINTS)
	{
		Word = Point/8;		// Which word of the mask we will be using
		Point -= Word*8;	// Which bit of the word we will be setting

		if(Mask[Word] & (uint8_t)(0x80>>Point))
			return true;
	}// If a valid point

	return false;
}// GetWPMaskBit


/*! Clear all the bits in the passed flight plan mask.
	\param Mask to be cleared. */
void ClearAllWPMaskBits(WPMask_t Mask)
{
	uint32_t i;

	for(i = 0; i < WP_MASK_BYTES; i++)
		Mask[i] = 0;
}// ClearAllWPMaskBits

/*! Set all the bits in the passed flight plan mask.
	\param Mask to be set. */
void SetAllWPMaskBits(WPMask_t Mask)
{
	uint32_t i;

	for(i = 0; i < WP_MASK_BYTES; i++)
		Mask[i] = 0xFF;
}// SetAllWPMaskBits

/*! Invert all bits in the flight plan mask.  Any bits which were set will be
	clear, and any bits which were clear will be set.
	\param Mask to be inverted. */
void InvertAllWPMaskBits(WPMask_t Mask)
{
	uint32_t i;
	for(i = 0; i < WP_MASK_BYTES; i++)
		Mask[i] = (uint8_t)~Mask[i];
}// InvertAllWPMaskBits


/*! Fill out the destination flight plan mask by inverting all bits in the
	source flight plan mask.  Any bits which were set will be clear, and any
	bits which were clear will be set.
	\param Dest is the new mask with inverted bits.
	\param Source is the source data that will remain unchanged. */
void InvertAllWPmaskBits_Ex(WPMask_t Dest, const WPMask_t Source)
{
	uint32_t i;
	for(i = 0; i < WP_MASK_BYTES; i++)
		Dest[i] = (uint8_t)~Source[i];
}// InvertAllWPMaskBits_Ex


/*! Copy the flight plan mask from the source to the destination.
	\param Source is the source mask.
	\param Destination is the destination mask.*/
void CopyWPMask(const WPMask_t Source, WPMask_t Destination)
{
	uint32_t i;
	for(i = 0; i < WP_MASK_BYTES; i++)
	{
		Destination[i] = Source[i];
	}// For all bytes in the mask

}// CopyWPMask


/*! Compare two flight plan masks.
	\param First is the first mask to compare.
	\param Second is the second mask to compare.
	\return 0 if the masks are identical.  If they are not then if the First
			mask has a lower number bit set that the Second does not the return
			is +1, else the return is -1.*/
int32_t CompareWPMask(const WPMask_t First, const WPMask_t Second)
{
	int i;
	for(i = 0; i < WP_MASK_BYTES; i++)
	{
		if(First[i] > Second[i])
			return 1;
		else if(First[i] > Second[i])
			return -1;
	}// For all bytes in the mask

	return 0;
}// CompareWPMask


/*! Bitwise OR all of the bits of the two flight plan masks.  The new mask is
	return in Mask1
	\param Mask1 is the first of the source masks, and also the return mask
	\param Mask2 is the second of the soruce masks, it will be unchanged. */
void WPMaskOR(WPMask_t Mask1, const WPMask_t Mask2)
{
	uint32_t i;
	for(i = 0; i < WP_MASK_BYTES; i++)
		Mask1[i] |= (uint8_t)Mask2[i];
}// WPMaskOR


/*! Bitwise OR all of the bits of the two flight plan masks.  The source masks
	are not changed.
	\param Return is the mask that contains the new data.
	\param Mask1 is the first of the source masks.
	\param Mask2 is the second of the soruce masks. */
void WPMaskOR_Ex(WPMask_t Return, const WPMask_t Mask1, const WPMask_t Mask2)
{
	uint32_t i;
	for(i = 0; i < WP_MASK_BYTES; i++)
		Return[i] = (uint8_t)(Mask1[i] | Mask2[i]);
}// WPMaskOR_Ex


/*! Bitwise AND all of the bits of the two flight plan masks.  The new mask is
	return in Mask1
	\param Mask1 is the first of the source masks, and also the return mask
	\param Mask2 is the second of the sANDuce masks, it will be unchanged. */
void WPMaskAND(WPMask_t Mask1, const WPMask_t Mask2)
{
	uint32_t i;
	for(i = 0; i < WP_MASK_BYTES; i++)
		Mask1[i] &= (uint8_t)Mask2[i];
}// WPMaskAND


/*! Bitwise AND all of the bits of the two flight plan masks.  The source masks
	are not changed.
	\param Return is the mask that contains the new data.
	\param Mask1 is the first of the source masks.
	\param Mask2 is the second of the sANDuce masks. */
void WPMaskAND_Ex(WPMask_t Return, const WPMask_t Mask1, const WPMask_t Mask2)
{
	uint32_t i;
	for(i = 0; i < WP_MASK_BYTES; i++)
		Return[i] = (uint8_t)(Mask1[i] & Mask2[i]);
}// WPMaskAND_Ex


/*! Search throught the flight plan mask, starting at index 0, looking for the
	first set bit.
	\param Mask to be searched.
	\return The index of the first bit which is set.  If no bits are set the
			return value is -1. */
int32_t FindFirstWPMaskBitSet(WPMask_t Mask)
{
	uint32_t i, j;
	int32_t Return = -1;

	for(i = 0; i < WP_MASK_BYTES; i++)
	{
		if(Mask[i])	// If a bit set
		{
			j = FindFirstBitSet(Mask[i]);	// Based on 32-bit field
			j -= 24;						      // Now based on 8 bit field

			Return = (int32_t)(i*8 + j);		// The bit which is set

			if(Return >= NUM_WP_POINTS)		// Catch any bits set past the limit
				Return = -1;

			break;
		}// If a bit set

	}// For all bytes in the mask

	return Return;
}// FindFirstWPMaskBitSet


/*! Search throught the flight plan mask, starting at one past the passed
	index, looking for the first set bit.
	\param Mask to be searched.
	\return The index of the lowest bit followin Index which is set.  If no
			bits are set the return value is -1. */
int32_t FindNextWPMaskBitSet(WPMask_t Mask, uint32_t Index)
{
	uint32_t Word;
	int32_t Return = -1;

	Index++;	// One more than the last one

	if(Index < NUM_WP_POINTS)
	{
		for(Word = Index/8; Word < WP_MASK_BYTES; Word++)
		{
			Index %= 8;	// Reset to modulo 8
			if(Mask[Word])
			{
				for(; Index < 8; Index++)
				{
					if(Mask[Word] & (uint8_t)(0x80>>Index))
					{
						Return = (int32_t)(Word*8 + Index);	// The index of the bit

						if(Return >= NUM_WP_POINTS)			// Catch any bits set past the limit
							Return = -1;

						return Return;
					}// If we found the bit which is set

				}// For all bits in this mask byte

			}// If any bits in this mask byte

		}// For all mask bytes that contain this and later indices

	}// If a valid Index

	return Return;
}// FindNextWPMaskBitSet

uint32_t FindFirstBitSet(uint32_t r)
// Returns the left based index of the most significant bit which is set
{
  uint32_t bit = 31;
  uint32_t temp1;

  if(r == 0) return 32; // No bits set

  temp1 = r & 0xFFFF0000;
  if(temp1)
  {
     bit -= 16; // Somewhere in MS half of word
     r = temp1; // Drop the LS half, since we know its in MS
  }// If in MS half of word

  temp1 = r & 0xFF00FF00;
  if(temp1)
  {
     bit -= 8;  // At least 8 closer to MS
     r = temp1; // Discard this segment
  }// If in next MS byte

  temp1 = r & 0xF0F0F0F0;
  if(temp1)
  {
     bit -= 4;  // At least 4 closer to MS
     r = temp1; // Discard this segment
  }// If in next MS nibble

  temp1 = r & 0xCCCCCCCC;
  if(temp1)
  {
     bit -= 2;  // At least 2 closer to MS
     r = temp1; // Discard this segment
  }// If in next MS two bits

  temp1 = r & 0xAAAAAAAA;
  if(temp1)
  {
     bit -= 1;  // At least 1 closer to MS
     r = temp1; // Discard this segment
  }// If in next MS bit

  // Note that bit value contains a number which represents

  return bit;
}// FindFirstBitSet

bool GPSWaypointPlan::clear_plan(int startInd)
{
	if( startInd >= NUM_WP_POINTS || !Plan[startInd].IsPointValid() )
	{
		cout << "!!! not a valid startInd in current plan to clear" << endl;
		return false;
	}

	WPMask_t Mask;
	int numPoints = GetPointsInPlan(Mask, startInd);
	if( numPoints <= 0 )
	{
		cout << "!!! no points in waypoint plan" << endl;
		return false;
	}

	ClearPointsFromPlan(Mask);
	return true;
}
